﻿using System;
using System.Collections.Generic;
using System.Linq;

public class DroneGeoreference
{
    // 无人机信息
    public double Roll { get; set; }
    public double Yaw { get; set; }
    public double Pitch { get; set; }
    public double Longitude { get; set; }
    public double Latitude { get; set; }
    public double Altitude { get; set; }
    public double RelativeAltitude { get; set; }

    // 云台信息
    public double GimbalRoll { get; set; }
    public double GimbalYaw { get; set; }
    public double GimbalPitch { get; set; }

    // 相机参数
    public double FocalLength { get; set; }
    public double PixelSize { get; set; }
    public int ImageWidth { get; set; }
    public int ImageHeight { get; set; }

    // 标定框坐标
    public Tuple<int, int> TopLeft { get; set; }
    public Tuple<int, int> BottomRight { get; set; }

    // DSM 数据 (用于提高精度)
    public Dictionary<Tuple<double, double>, double> DSM { get; set; }

    public DroneGeoreference(double roll, double yaw, double pitch, double longitude, double latitude,
                             double altitude, double relativeAltitude, double gimbalRoll, double gimbalYaw,
                             double gimbalPitch, double focalLength, double pixelSize, int imageWidth,
                             int imageHeight, Tuple<int, int> topLeft, Tuple<int, int> bottomRight)
    {
        Roll = roll;
        Yaw = yaw;
        Pitch = pitch;
        Longitude = longitude;
        Latitude = latitude;
        Altitude = altitude;
        RelativeAltitude = relativeAltitude;
        GimbalRoll = gimbalRoll;
        GimbalYaw = gimbalYaw;
        GimbalPitch = gimbalPitch;
        FocalLength = focalLength;
        PixelSize = pixelSize;
        ImageWidth = imageWidth;
        ImageHeight = imageHeight;
        TopLeft = topLeft;
        BottomRight = bottomRight;
        DSM = new Dictionary<Tuple<double, double>, double>();
    }

    // 转换弧度到角度
    private double ToDegrees(double radians)
    {
        return radians * (180.0 / Math.PI);
    }

    // 转换角度到弧度
    private double ToRadians(double degrees)
    {
        return degrees * (Math.PI / 180.0);
    }

    // 地理坐标计算
    private double[] ComputeGroundCoordinates(int x, int y, double altitude)
    {
        // 计算基于像元尺寸和焦距的地面坐标变化量
        double xGround = (x - ImageWidth / 2) * PixelSize * (altitude / FocalLength);
        double yGround = (y - ImageHeight / 2) * PixelSize * (altitude / FocalLength);
        return new double[] { xGround, yGround };
    }

    // 计算图片中心的经纬度
    public (double latitude, double longitude) GetImageCenterCoordinates()
    {
        // 假设图片中心是相机视线的交点
        double[] centerGround = ComputeGroundCoordinates(ImageWidth / 2, ImageHeight / 2, Altitude);

        // 计算图像中心对应的经纬度变化量
        double dLat = centerGround[1] / 111000.0; // 纬度变化对应的地面距离
        double dLon = centerGround[0] / (Math.Cos(ToRadians(Latitude)) * 111000.0); // 经度变化对应的地面距离

        double newLat = Latitude + dLat;
        double newLon = Longitude + dLon;

        return (newLat, newLon);
    }

    // 计算标注物体的经纬度
    public (double latitude, double longitude) GetTargetObjectCoordinates()
    {
        double[] topLeftGround = ComputeGroundCoordinates(TopLeft.Item1, TopLeft.Item2, Altitude);
        double[] bottomRightGround = ComputeGroundCoordinates(BottomRight.Item1, BottomRight.Item2, Altitude);

        // 计算标定框中心位置
        double xGround = (topLeftGround[0] + bottomRightGround[0]) / 2;
        double yGround = (topLeftGround[1] + bottomRightGround[1]) / 2;

        // 将地面坐标转换为经纬度
        double dLat = yGround / 111000.0;
        double dLon = xGround / (Math.Cos(ToRadians(Latitude)) * 111000.0);

        double targetLat = Latitude + dLat;
        double targetLon = Longitude + dLon;

        return (targetLat, targetLon);
    }

    public static void LMain()
    {
        var drone = new DroneGeoreference(
            roll: 15, yaw: 88.90, pitch: -18.4,
            longitude: 100.20005, latitude: 36.003380555555559,
            altitude: 3109.71, relativeAltitude: 69.662,
            gimbalRoll: 180, gimbalYaw: -89.30, gimbalPitch: -90,
            focalLength: 24, pixelSize: 12e-6, imageWidth: 640,
            imageHeight: 512, topLeft: Tuple.Create(169, 253),
            bottomRight: Tuple.Create(192, 296)
        );

        var centerCoordinates = drone.GetImageCenterCoordinates();
        var targetCoordinates = drone.GetTargetObjectCoordinates();

        Console.WriteLine($"Image Center Coordinates: Latitude = {centerCoordinates.latitude}, Longitude = {centerCoordinates.longitude}");
        Console.WriteLine($"Target Object Coordinates: Latitude = {targetCoordinates.latitude}, Longitude = {targetCoordinates.longitude}");
    }
    public static void LMain1()
    {
        var drone = new DroneGeoreference(
            roll: -1.90, yaw: 0, pitch: -3.60,
            longitude: 124.87156111111111, latitude: 46.147141666666663,
            altitude: 370, relativeAltitude: 47.044,
            gimbalRoll: 0, gimbalYaw: -1.50, gimbalPitch: -45,
            focalLength: 24, pixelSize: 12e-6, imageWidth: 1280,
            imageHeight: 1024, topLeft: Tuple.Create(260, 534),
            bottomRight: Tuple.Create(350, 588)
        );

        var centerCoordinates = drone.GetImageCenterCoordinates();
        var targetCoordinates = drone.GetTargetObjectCoordinates();

        Console.WriteLine($"Image Center Coordinates: Latitude = {centerCoordinates.latitude}, Longitude = {centerCoordinates.longitude}");
        Console.WriteLine($"Target Object Coordinates: Latitude = {targetCoordinates.latitude}, Longitude = {targetCoordinates.longitude}");
    }
}


